/**
  * @file example_getRectFrame.cc
  * @brief This file is part of UnitreeCameraSDK.
  * @details This example that how to Transmission picture
  * @author SunMingzhe
  * @date  2021.12.07
  * @version 1.1.0
  * @copyright Copyright (c) 2020-2021, Hangzhou Yushu Technology Stock CO.LTD. All Rights Reserved.
  */


/*
发送端：
方法概述：采用udp方法输出图片，传输方式已写入程序，由StereoCameraCommon::startCapture(true,false)中第一个参数控制定向udp传输，当其为true时打开udp264硬编码传输模式
参数：配置文件中IpLastSegment是接收端ip，接收端地址必须是192.168.123.IpLastSegment。
     配置文件中Transmode决定传输模式
     配置文件中Transrate决定传输速率，限制了传输速率小于图片速率
     端口9201~9205分别对应前方，下巴，左，右，腹部
     启动自定义图传时需使用kill.sh文件关闭Unitree/autostart/02camerarosnode程序来解除相机占用，Unitree/autostart/04imageai来释放算力。
     （也可使用以下几条指令
     ps -A | grep point | awk '{print $1}' | xargs kill -9
     ps -aux|grep mqttControlNode|grep -v grep|head -n 1|awk '{print $2}'|xargs kill -9
     ps -aux|grep live_human_pose|grep -v grep|head -n 1|awk '{print $2}'|xargs kill -9
     ）
传输图片模式：由配置文件中Transmode决定，该值只可通过读取配置文件方式写入
0：传输左目原始图片
1：传输双目原始图片
2：传输左目畸变校正后图片，视野范围角度可调60~140
3：传输双目畸变校正后图片
※※※注意，双目图的最后的左右目输出位置是反的。左目->右图，右目->左图
4：不建议使用！！，Depthmode = 2时 可输出深度图和对应左目图片，不建议使用。
*/


#include <UnitreeCameraSDK.hpp>
#include <unistd.h>

int main(int argc, char *argv[])
{
    int cam_num = 1;
    if (argc>=2)
        cam_num = std::atoi(argv[1]);

    // int cam_num = 1; // 1,2 the number of cameras used
    // nano_id dev_id   port_id   位置
    //   13       0      9202     下巴
    //   13       1      9201     前方
    //   14       0      9203     左方
    //   14       1      9204     右方
    //   15       0      9205     腹部（默认）
    int cam_id = 0; // the id of the camera used if cam_num is 1
    if (cam_num == 1){
        UnitreeCamera cam("trans_rect_config_"+ std::to_string(cam_id) +".yaml"); ///< init camera by device node number
        if(!cam.isOpened())   ///< get camera open state
            exit(EXIT_FAILURE);   
        cam.startCapture(true,false); ///< disable share memory sharing and able image h264 encoding

        usleep(500000);
        while(cam.isOpened())
        {
            cv::Mat left,right,feim;
            if(!cam.getRectStereoFrame(left,right))
            {
                
                usleep(1000);
                continue;
            }
            char key = cv::waitKey(1);
            if(key == 27) // press ESC key
            break;
        }
        
        cam.stopCapture(); ///< stop camera capturing
    }
    else {
        UnitreeCamera cam1("trans_rect_config_0.yaml"); 
        UnitreeCamera cam2("trans_rect_config_1.yaml");
        if(!cam1.isOpened()) 
            exit(EXIT_FAILURE);   
        if(!cam2.isOpened())
            exit(EXIT_FAILURE);   
        cam1.startCapture(true,false); 
        cam2.startCapture(true,false); 
        
        usleep(500000);
        while(cam1.isOpened())
        {
            cv::Mat left1,right1,feim1,left2,right2,feim2;
            if(!cam1.getRectStereoFrame(left1,right1))
            {
                usleep(500);
                continue;
            }
            if(!cam2.getRectStereoFrame(left2,right2))
            {
                usleep(500);
                continue;
            }
            char key = cv::waitKey(1);
            if(key == 27) // press ESC key
            break;
        }
    
    
    }
    
    return 0;
}
